function idat = fsb_realign_2D_ctm(idat,disp2)

% FSB : Realign each slice to mean slice with Center of Mass algorithm
%
% EXAMPLE:
% idat = fsb_realign_2D_ctm(idat,1)
%
% INPUT:
% idat:         4-D image data
% ref:          Reference slice
%
% OUTPUT:
% A coarsely realigned 4-D image data volume
%
% CALLED BY:
% fsb_realign.m
%
% NOTES:
% Very coarse realignment, indeed...
% Speed was the main goal here.
% As the algorithm averages between successive volumes, there is some
% additional smoothness introduced in the images.
% Useful for realignment of volumes that are not in need for
% rotation or shifts in other directions.
% Steffen Stoewer 28/11/07
% Center of mass code by George A Keliris 2006
%
% Copyright 2010 MPI for Biological Cybernetics
% Author: Steffen Stoewer
% License: GNU GPL, no express or implied warranties
% 
% $Revision 1.0
%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

tic;

[a,b,c,d]=size(idat);%get idat parameters

%~~~~~~~~~~~~~~~~~~~~~
% Create indices
%~~~~~~~~~~~~~~~~~~~~~
[ix iy]=find(ones(a,b));
linear_ind=find(ones(a,b));

%~~~~~~~~~~~~~~~~~~~~~
% Perform Center of Mass calculation over slices
%~~~~~~~~~~~~~~~~~~~~~
h = waitbar(0,'Determining 2D center of mass...');
ctr_mass_xyz= zeros(c,d);
for z=1:c;
    for t=1:d
        img=double(idat(:,:,z,t));
        w=img(linear_ind);
        waitbar(z/c);
        ctr_mass_xyz(z,t)=sum((w.*iy)/sum(w));
    end
end
close(h);

%~~~~~~~~~~~~~~~~~~~~~
% Calculate average shift per slice and round every shift towards zero
%~~~~~~~~~~~~~~~~~~~~~
slice_shift=nanmean(ctr_mass_xyz,2);
dctr_mass_r2=ctr_mass_xyz-repmat(slice_shift,1,d);
dc = fix(dctr_mass_r2);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%Use calculated parameters for realignment
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
h = waitbar(0,'Realigning...');

for z=1:c;

    for x = 1:d

        %~~~~~~~~~~~~~~~~~~~~~
        % Determine shift direction, and calculate voxel fraction
        %~~~~~~~~~~~~~~~~~~~~~
        dc_rem(z,x) = dctr_mass_r2(z,x)-dc(z,x);

        %~~~~~~~~~~~~~~~~~~~~~
        % Create dummy slice
        %~~~~~~~~~~~~~~~~~~~~~
        p_idat = idat(:,:,z,x);

        %~~~~~~~~~~~~~~~~~~~~~
        % Shift volume if shift >= 1 Voxel
        %~~~~~~~~~~~~~~~~~~~~~
        if dc(z,x)~=0 && ~isnan(dc(z,x));
            p_idat = circshift(p_idat,[0 -dc(z,x)]);
        end

        %~~~~~~~~~~~~~~~~~~~~~
        % create shifted arrays
        %~~~~~~~~~~~~~~~~~~~~~
        p_idat1 = circshift(p_idat,[0 1]);
        p_idat2 = circshift (p_idat,[0 -1]);

        %~~~~~~~~~~~~~~~~~~~~~
        % shift volume by fraction
        %~~~~~~~~~~~~~~~~~~~~~
        if dc_rem(z,x)>0;
            p_idat = (p_idat2*dc_rem(z,x))+(p_idat*(1-(dc_rem(z,x))));
        elseif dc_rem(z,x)<0;
            p_idat = (p_idat1*abs(dc_rem(z,x)))+(p_idat*(1-abs(dc_rem(z,x))));
        end

        %~~~~~~~~~~~~~~~~~~~~~
        % write into new idat
        %~~~~~~~~~~~~~~~~~~~~~
        idat(:,:,z,x) = p_idat;

        waitbar(z/c);
    end
end

close(h);
toc;

if disp2 ==1;
    figure(500);
    plot(dctr_mass_r2);
    xlabel('time -> [img]');
    ylabel('center-of-mass shift -> [voxel]')
end
end
